;********************************************************************************************************
;                                              uC/OS-II
;                                        The Real-Time Kernel
;
;                    Copyright 1992-2021 Silicon Laboratories Inc. www.silabs.com
;
;                                 SPDX-License-Identifier: APACHE-2.0
;
;               This software is subject to an open source license and is distributed by
;                Silicon Laboratories Inc. pursuant to the terms of the Apache License,
;                    Version 2.0 available at www.apache.org/licenses/LICENSE-2.0.
;
;********************************************************************************************************

;********************************************************************************************************
;
;                                             ARMv7-M Port
;
; Filename  : os_cpu_a.asm
; Version   : V2.93.01
;********************************************************************************************************
; For       : ARMv7-M Cortex-M
; Mode      : Thumb-2 ISA
; Toolchain : ARM C Compiler
;********************************************************************************************************
; Note(s)   : (1) This port supports the ARM Cortex-M3, Cortex-M4 and Cortex-M7 architectures.
;             (2) It has been tested with the following Hardware Floating Point Unit.
;                 (a) Single-precision: FPv4-SP-D16-M and FPv5-SP-D16-M
;                 (b) Double-precision: FPv5-D16-M
;********************************************************************************************************

;********************************************************************************************************
;                                          PUBLIC FUNCTIONS
;********************************************************************************************************

    EXTERN  OSRunning                                           ; External references
    EXTERN  OSPrioCur
    EXTERN  OSPrioHighRdy
    EXTERN  OSTCBCur
    EXTERN  OSTCBHighRdy
    EXTERN  OSIntExit
    EXTERN  OSTaskSwHook
    EXTERN  OS_CPU_ExceptStkBase
    EXTERN  OS_KA_BASEPRI_Boundary
	EXTERN  OSIntNesting


    EXPORT  OSStartHighRdy                                      ; Functions declared in this file
    EXPORT  OS_CPU_SR_Save
    EXPORT  OS_CPU_SR_Restore
    EXPORT  OSCtxSw
    EXPORT  OSIntCtxSw
    EXPORT  OS_CPU_PendSVHandler

    IF {FPU} != "SoftVFP"
    EXPORT  OS_CPU_FP_Reg_Push
    EXPORT  OS_CPU_FP_Reg_Pop
    ENDIF


;********************************************************************************************************
;                                               EQUATES
;********************************************************************************************************

NVIC_INT_CTRL   EQU     0xE000ED04                              ; Interrupt control state register.
NVIC_SYSPRI14   EQU     0xE000ED22                              ; System priority register (priority 14).
NVIC_PENDSV_PRI EQU           0xFF                              ; PendSV priority value (lowest).
NVIC_PENDSVSET  EQU     0x10000000                              ; Value to trigger PendSV exception.
LR_RETURN 		EQU		0XFFFFFFED								; Value to return to Thread Mode and use PSP(always)



;********************************************************************************************************
;                                     CODE GENERATION DIRECTIVES
;********************************************************************************************************

    AREA |.text|, CODE, READONLY, ALIGN=2
    THUMB
    REQUIRE8
    PRESERVE8


;********************************************************************************************************
;                                   FLOATING POINT REGISTERS PUSH
;                             void  OS_CPU_FP_Reg_Push (OS_STK  *stkPtr)
;
; Note(s) : 1) This function saves S16-S31 registers of the Floating Point Unit.
;
;           2) Pseudo-code is:
;              a) Push remaining FPU regs S16-S31 on process stack;
;              b) Update OSTCBCur->OSTCBStkPtr;
;********************************************************************************************************

    IF {FPU} != "SoftVFP"

OS_CPU_FP_Reg_Push
    MRS     R1, PSP                                             ; PSP is process stack pointer
    CBZ     R1, OS_CPU_FP_nosave                                ; Skip FP register save the first time
	;CBZ指令是一个条件分支跳转指令，若比较为0，则跳转到对应地址去

    VSTMDB  R0!, {S16-S31}
    LDR     R1, =OSTCBCur
    LDR     R2, [R1]
    STR     R0, [R2]
OS_CPU_FP_nosave
    BX      LR

    ENDIF


;********************************************************************************************************
;                                   FLOATING POINT REGISTERS POP
;                             void  OS_CPU_FP_Reg_Pop (OS_STK  *stkPtr)
;
; Note(s) : 1) This function restores S16-S31 of the Floating Point Unit.
;
;           2) Pseudo-code is:
;              a) Restore regs S16-S31 of new process stack;
;              b) Update OSTCBHighRdy->OSTCBStkPtr pointer of new proces stack;
;********************************************************************************************************

    IF {FPU} != "SoftVFP"

OS_CPU_FP_Reg_Pop
	MRS R1,PSP
	CBZ R1,OS_CPU_FP_nopop
	
    VLDMIA  R0!, {S16-S31}
    LDR     R1, =OSTCBHighRdy
    LDR     R2, [R1]
    STR     R0, [R2]
OS_CPU_FP_nopop
	BX      LR

    ENDIF


;********************************************************************************************************
;                                   CRITICAL SECTION METHOD 3 FUNCTIONS
;
; Description : Disable/Enable Kernel aware interrupts by preserving the state of BASEPRI.  Generally speaking,
;               the state of the BASEPRI interrupt exception processing is stored in the local variable
;               'cpu_sr' & Kernel Aware interrupts are then disabled ('cpu_sr' is allocated in all functions
;               that need to disable Kernel aware interrupts). The previous BASEPRI interrupt state is restored
;               by copying 'cpu_sr' into the BASEPRI register.
;
; Prototypes  : OS_CPU_SR  OS_CPU_SR_Save   (OS_CPU_SR  new_basepri);
;               void       OS_CPU_SR_Restore(OS_CPU_SR  cpu_sr);
;
;
; Note(s)     : 1) These functions are used in general like this:
;
;                  void Task (void *p_arg)
;                  {
;                  #if OS_CRITICAL_METHOD == 3          /* Allocate storage for CPU status register  */
;                      OS_CPU_SR  cpu_sr;
;                  #endif
;
;                          :
;                          :
;                      OS_ENTER_CRITICAL();             /* cpu_sr = OS_CPU_SR_Save(new_basepri);     */
;                          :
;                          :
;                      OS_EXIT_CRITICAL();              /* OS_CPU_RestoreSR(cpu_sr);                 */
;                          :
;                          :
;                  }
;
;               2) Increasing priority using a write to BASEPRI does not take effect immediately.
;                  (a) IMPLICATION  This erratum means that the instruction after an MSR to boost BASEPRI
;                      might incorrectly be preempted by an insufficient high priority exception.
;
;                  (b) WORKAROUND  The MSR to boost BASEPRI can be replaced by the following code sequence:
;
;                      CPSID i
;                      MSR to BASEPRI
;                      DSB
;                      ISB
;                      CPSIE i
;********************************************************************************************************

;OS_CPU_SR_Save
	;CPSID I                 ;关闭中断，I位只是关中断，F位只是关快中断，IF则是中断和快中断都关，但是在ARM cortex-M系列由于提供了另一种机制
							;;所以去除了FIQ模式，也就不需要都关
	;PUSH {R1}				;压栈，我们使用R1作为一个中介
	;MRS R1,BASEPRI			;当前状态(被中断的任务/被嵌套的中断)中BASEPRI,与S3C2440直接保存PSR不同
	;MSR BASEPRI,R0			;由于我们传参进来，所以需要通过R0把参数给BASEPRI
	;DSB						
	;ISB
	;MOV R0,R1				;把原来状态的BASEPRI作为返回值传给当前中断的cpu_sr
	;POP {R1}				;出栈
	;CPSIE I					;开中断
	;BX LR					;返回执行
	
;OS_CPU_SR_Restore
	;CPSID I
	;MSR BASEPRI,R0			;这里我们使用当前中断的cpu_sr作为参数传递以恢复之前的BASEPRI，与S3C2440恢复PSR不同
	;DSB
	;ISB
	;CPSIE I
	;BX LR
	
OS_CPU_SR_Save
    MRS     R0, PRIMASK                                         ; Set prio int mask to mask all (except faults)
    CPSID   I
    BX      LR

OS_CPU_SR_Restore
    MSR     PRIMASK, R0
    BX      LR

;********************************************************************************************************
;                                         START MULTITASKING
;                                      void OSStartHighRdy(void)
;
; Note(s) : 1) This function triggers a PendSV exception (essentially, causes a context switch) to cause
;              the first task to start.
;
;           2) During task execution, PSP is used as the stack pointer.
;              When an exception occurs, the core will switch to MSP until the exception return.
;
;           3) OSStartHighRdy() MUST:
;              a) Setup PendSV exception priority to lowest;
;              b) Set initial PSP to 0, to tell context switcher this is first run;
;              c) Set the main stack to OS_CPU_ExceptStkBase
;              d) Set OSRunning to TRUE;
;              e) Get current high priority, OSPrioCur = OSPrioHighRdy;
;              f) Get current ready thread TCB, OSTCBCur = OSTCBHighRdy;
;              g) Get new process SP from TCB, SP = OSTCBHighRdy->OSTCBStkPtr;
;              h) Restore R0-R11 and R14 from new process stack;
;              i) Enable interrupts (tasks will run with interrupts enabled).
;********************************************************************************************************

OSStartHighRdy
	
;初始化MSP.PSP.PendSV的优先级，定义PendSV为最低优先级
;这里解释一下，由于CortexM4里面的优先级寄存器分为了抢占和响应两种，共用四位
	LDR R0,=NVIC_SYSPRI14
	MOV R1,#NVIC_PENDSV_PRI
	STRB R1,[R0] ;定义抢占优先级为15，也就是所能分配的最低优先级
	
	;初始化PSP为0，以告知Switcher这是第一次任务开始
	MOV R0,#0
	MSR PSP,R0
	
	;初始化MSP为OS_CPU_ExceptStkBase
	LDR R0,=OS_CPU_ExceptStkBase
	LDR R0,[R0]
	MSR MSP,R0
	
	;BL OSTaskSwHook ;这条指令其实本来带有更新MSP的作用，但是我们其实没有必要去进行这条指令做一些无所谓的跳转和操作，所以这里需要自己手动更新PSP
	;因为里面有一个FP_POP这会更新当前使用的栈指针
	
	;设置OSRunning为TRUE
	LDR R0, =OSRunning
	MOV R1, #1
	STRB R1, [R0]
	
	;我们在前面的步骤已经确定了OSPrioCur和OSTCBCur
	;我们取出我们自己定义的栈的地址赋给PSP，以作为接下来的操作的栈指针
	LDR R0,=OSTCBCur
	LDR R0,[R0]
	LDR R0,[R0]
	MSR PSP,R0
	
	;由于正常情况下两种程序运行模式都是默认使用MSP，但是正常程序运行最好使用PSP
	;默认上电为特权模式，只要不去特意改变，就会一直处在特权模式
	MRS R0,CONTROL
	ORR R0,R0,#0x02 ;使用PSP
	MSR CONTROL,R0
	ISB
	;从这里开始，我们就正式开始使用PSP指针了
	;初始化之后由于是初次开始，要进行以此特殊的上下文切换
	;让第一个任务开始执行，这个函数不是异常，所以硬件不会自动帮我们压栈，需要在这里直接手动进行压栈
	LDMFD SP!,{R4-R11}
	LDMFD SP!,{R0-R3}
	LDMFD SP!,{R12,LR}
	LDMFD SP!,{R1,R2};R1存储了PC，R2存储了CPSR的值
	;按照我的理解这里甚至只需要把PC读出来就行了。没有这么多繁琐的步骤，因为我们对第一个任务没有什么特殊的需求
	
	;PUSH    {R14}                                               ; Save LR exc_return value
    ;BL OSTaskSwHook
    ;POP     {R14}
	;;新增
	;VLDMIA SP!,{S16-S31}
	;PC赋值
	BX R1 ;由于在最开始的时候我们模拟压栈时就确定了程序入口，所以出栈给PC可以直接开始执行，由于正常函数执行也不是异常，所以选出的程序处于Thread模式下的特权模式


;********************************************************************************************************
;                       PERFORM A CONTEXT SWITCH (From task level) - OSCtxSw()
;                   PERFORM A CONTEXT SWITCH (From interrupt level) - OSIntCtxSw()
;
; Note(s) : 1) OSCtxSw() is called when OS wants to perform a task context switch.  This function
;              triggers the PendSV exception which is where the real work is done.
;
;           2) OSIntCtxSw() is called by OSIntExit() when it determines a context switch is needed as
;              the result of an interrupt.  This function simply triggers a PendSV exception which will
;              be handled when there are no more interrupts active and interrupts are enabled.
;********************************************************************************************************

;OSCtxSw
;OSIntCtxSw
    ;LDR     R0, =NVIC_INT_CTRL                                  ; Trigger the PendSV exception (causes context switch)
    ;LDR     R1, =NVIC_PENDSVSET
    ;STR     R1, [R0]
    ;BX      LR
OSCtxSw
OSIntCtxSw
    LDR     R0, =NVIC_INT_CTRL                                  ; Trigger the PendSV exception (causes context switch)
    LDR     R1, =NVIC_PENDSVSET
    STR     R1, [R0]
	DSB		
	ISB
    BX      LR

;********************************************************************************************************
;                                       HANDLE PendSV EXCEPTION
;                                   void OS_CPU_PendSVHandler(void)
;
; Note(s) : 1) PendSV is used to cause a context switch.  This is a recommended method for performing
;              context switches with Cortex-M.  This is because the Cortex-M auto-saves half of the
;              processor context on any exception, and restores same on return from exception.  So only
;              saving of R4-R11 & R14 is required and fixing up the stack pointers. Using the PendSV exception
;              this way means that context saving and restoring is identical whether it is initiated from
;              a thread or occurs due to an interrupt or exception.
;
;           2) Pseudo-code is:
;              a) Get the process SP
;              b) Save remaining regs r4-r11 & r14 on process stack;
;              c) Save the process SP in its TCB, OSTCBCur->OSTCBStkPtr = SP;
;              d) Call OSTaskSwHook();
;              e) Get current high priority, OSPrioCur = OSPrioHighRdy;
;              f) Get current ready thread TCB, OSTCBCur = OSTCBHighRdy;
;              g) Get new process SP from TCB, SP = OSTCBHighRdy->OSTCBStkPtr;
;              h) Restore R4-R11 and R14 from new process stack;
;              i) Perform exception return which will restore remaining context.
;
;           3) On entry into PendSV handler:
;              a) The following have been saved on the process stack (by processor):
;                 xPSR, PC, LR, R12, R0-R3
;              b) Processor mode is switched to Handler mode (from Thread mode)
;              c) Stack is Main stack (switched from Process stack)
;              d) OSTCBCur      points to the OS_TCB of the task to suspend
;                 OSTCBHighRdy  points to the OS_TCB of the task to resume
;
;           4) Since PendSV is set to lowest priority in the system (by OSStartHighRdy() above), we
;              know that it will only be run when no other exception or interrupt is active, and
;              therefore safe to assume that context being switched out was using the process stack (PSP).
;
;           5) Increasing priority using a write to BASEPRI does not take effect immediately.
;              (a) IMPLICATION  This erratum means that the instruction after an MSR to boost BASEPRI
;                  might incorrectly be preempted by an insufficient high priority exception.
;
;              (b) WORKAROUND  The MSR to boost BASEPRI can be replaced by the following code sequence:
;
;                  CPSID i
;                  MSR to BASEPRI
;                  DSB
;                  ISB
;                  CPSIE i
;********************************************************************************************************

OS_CPU_PendSVHandler ;由于不论是主动切换还是被动切换，都会经过这个PendSV来进行处理，并且通过资料我们可以知道，进入中断之前我们的硬件就已经把相关寄存器存入了被中断的例程所使用的指针所指向的内存
	MRS R0,PSP
	STMFD R0!,{R4-R11};保存剩余现场,这里就包含了EXC_RETURN，在退出异常时，可以确定接下来的模式和栈指针
	
	LDR R2,=OSTCBCur
	LDR R1,[R2]
	STR R0,[R1];当前状态下，R0就是SP，需要存入TCB块
	
	;在这里维护EXC_RETURN
	;MOV R4,LR
	;BL OSTaskSwHook
	;ORR LR,R4,#0x04 ;保证异常返回使用PSP
	
	;然后切换OSTCBCur
	;①进行OSPrioCur的切换，由于我们之前已经通过OS_SchedNew()改变了当前的OSPrioHighRdy
	;OSPrioCur = OSPrioHighRdy
	LDR R0,=OSPrioCur
	LDR R1,=OSPrioHighRdy
	LDRB R3,[R1];为什么在此处用LDRB而不是LDR呢，主要是因为优先级都是INT8U，为了防止其扩展为32位读取其他区域，我们采取字节的方式写入
	STRB R3,[R0];同理
	
	;②我们进行OSTCBCur的改变，同样此处的OSTCBHighRdy已经在OS_Sched()的前面步骤中获得
	;但是为了后续的切换，每一次还是需要更新OSTCBCur的
	LDR R0,=OSTCBCur
	LDR R1,=OSTCBHighRdy
	LDR R2,[R1]
	STR R2,[R0]
	
	;然后获取当前最高优先级任务的栈指针
	LDR R0,[R0]
	LDR R0,[R0]
	
	 
	
	;然后正式开始恢复现场，也就是接下来的任务的现场恢复
	LDMIA R0!,{R4-R11};异常中也就不需要进行对剩余的8个寄存器进行手动出栈了，硬件帮助我们完成
	MSR PSP,R0;关于这里为什么不存一下到OSTCBCur->OSTCBStkPtr，可以看到我们上面进入的时候就是直接使用的PSP，也没有找OSTCBCur的首个字段值，所以我们在这里也就不需要去存
			  ;在后续的操作结束后呢，我们默认其PSP恰好回到了我们恢复时的位置
	
	PUSH    {LR}                                               ; Save LR exc_return value
    BL OSTaskSwHook
    POP     {LR}
	
	ORR LR,LR,#0x04											;让LR+4，如果压推栈了是否还需要？
	
	BX LR
	
    ALIGN                                                       ; Removes warning[A1581W]: added <no_padbytes> of padding at <address>

    END
;OS_CPU_PendSVHandler
    ;CPSID   I                                                   ; Prevent interruption during context switch
    ;MRS     R0, PSP                                             ; PSP is process stack pointer
    ;CBZ     R0, OS_CPU_PendSVHandler_nosave                     ; Skip register save the first time

    ;SUBS    R0, R0, #0x20                                       ; Save remaining regs r4-11 on process stack
    ;STM     R0, {R4-R11}

    ;LDR     R1, =OSTCBCur                                       ; OSTCBCur->OSTCBStkPtr = SP;
    ;LDR     R1, [R1]
    ;STR     R0, [R1]                                            ; R0 is SP of process being switched out

                                                                ;; At this point, entire context of process has been saved
;OS_CPU_PendSVHandler_nosave
    ;PUSH    {R14}                                               ; Save LR exc_return value
    ;LDR     R0, =OSTaskSwHook                                   ; OSTaskSwHook();
    ;BLX     R0
    ;POP     {R14}

    ;LDR     R0, =OSPrioCur                                      ; OSPrioCur = OSPrioHighRdy;
    ;LDR     R1, =OSPrioHighRdy
    ;LDRB    R2, [R1]
    ;STRB    R2, [R0]

    ;LDR     R0, =OSTCBCur                                       ; OSTCBCur  = OSTCBHighRdy;
    ;LDR     R1, =OSTCBHighRdy
    ;LDR     R2, [R1]
    ;STR     R2, [R0]

    ;LDR     R0, [R2]                                            ; R0 is new process SP; SP = OSTCBHighRdy->OSTCBStkPtr;
    ;LDM     R0, {R4-R11}                                        ; Restore r4-11 from new process stack
    ;ADDS    R0, R0, #0x20
    ;MSR     PSP, R0                                             ; Load PSP with new process SP
    ;ORR     LR, LR, #0x04                                       ; Ensure exception return uses process stack
    ;CPSIE   I
    ;BX      LR                                                  ; Exception return will restore remaining context

    ;END
	
	;;********************************************************************************************************
;;                                              uC/OS-II
;;                                        The Real-Time Kernel
;;
;;                    Copyright 1992-2021 Silicon Laboratories Inc. www.silabs.com
;;
;;                                 SPDX-License-Identifier: APACHE-2.0
;;
;;               This software is subject to an open source license and is distributed by
;;                Silicon Laboratories Inc. pursuant to the terms of the Apache License,
;;                    Version 2.0 available at www.apache.org/licenses/LICENSE-2.0.
;;
;;********************************************************************************************************

;;********************************************************************************************************
;;
;;                                             ARMv7-M Port
;;
;; Filename  : os_cpu_a.asm
;; Version   : V2.93.01
;;********************************************************************************************************
;; For       : ARMv7-M Cortex-M
;; Mode      : Thumb-2 ISA
;; Toolchain : ARM C Compiler
;;********************************************************************************************************
;; Note(s)   : (1) This port supports the ARM Cortex-M3, Cortex-M4 and Cortex-M7 architectures.
;;             (2) It has been tested with the following Hardware Floating Point Unit.
;;                 (a) Single-precision: FPv4-SP-D16-M and FPv5-SP-D16-M
;;                 (b) Double-precision: FPv5-D16-M
;;********************************************************************************************************

;;********************************************************************************************************
;;                                          PUBLIC FUNCTIONS
;;********************************************************************************************************

    ;EXTERN  OSRunning                                           ; External references
    ;EXTERN  OSPrioCur
    ;EXTERN  OSPrioHighRdy
    ;EXTERN  OSTCBCur
    ;EXTERN  OSTCBHighRdy
    ;EXTERN  OSIntExit
    ;EXTERN  OSTaskSwHook
    ;EXTERN  OS_CPU_ExceptStkBase
    ;EXTERN  OS_KA_BASEPRI_Boundary
	;EXTERN  OSIntNesting


    ;EXPORT  OSStartHighRdy                                      ; Functions declared in this file
    ;EXPORT  OS_CPU_SR_Save
    ;EXPORT  OS_CPU_SR_Restore
    ;EXPORT  OSCtxSw
    ;EXPORT  OSIntCtxSw
    ;EXPORT  OS_CPU_PendSVHandler

    ;IF {FPU} != "SoftVFP"
    ;EXPORT  OS_CPU_FP_Reg_Push
    ;EXPORT  OS_CPU_FP_Reg_Pop
    ;ENDIF


;;********************************************************************************************************
;;                                               EQUATES
;;********************************************************************************************************

;NVIC_INT_CTRL   EQU     0xE000ED04                              ; Interrupt control state register.
;NVIC_SYSPRI14   EQU     0xE000ED22                              ; System priority register (priority 14).
;NVIC_PENDSV_PRI EQU           0xFF                              ; PendSV priority value (lowest).
;NVIC_PENDSVSET  EQU     0x10000000                              ; Value to trigger PendSV exception.
;LR_RETURN 		  EQU		  0XFFFFFFED								; Value to return to Thread Mode and use PSP(always)



;;********************************************************************************************************
;;                                     CODE GENERATION DIRECTIVES
;;********************************************************************************************************

    ;AREA |.text|, CODE, READONLY, ALIGN=2
    ;THUMB
    ;REQUIRE8
    ;PRESERVE8


;;********************************************************************************************************
;;                                   FLOATING POINT REGISTERS PUSH
;;                             void  OS_CPU_FP_Reg_Push (OS_STK  *stkPtr)
;;
;; Note(s) : 1) This function saves S16-S31 registers of the Floating Point Unit.
;;
;;           2) Pseudo-code is:
;;              a) Push remaining FPU regs S16-S31 on process stack;
;;              b) Update OSTCBCur->OSTCBStkPtr;
;;********************************************************************************************************

    ;IF {FPU} != "SoftVFP"

;OS_CPU_FP_Reg_Push
    ;MRS     R1, PSP                                             ; PSP is process stack pointer
    ;CBZ     R1, OS_CPU_FP_nosave                                ; Skip FP register save the first time
	;;CBZ指令是一个条件分支跳转指令，若比较为0，则跳转到对应地址去

    ;VSTMDB  R0!, {S16-S31}
    ;LDR     R1, =OSTCBCur
    ;LDR     R2, [R1]
    ;STR     R0, [R2]
;OS_CPU_FP_nosave
    ;BX      LR

    ;ENDIF


;;********************************************************************************************************
;;                                   FLOATING POINT REGISTERS POP
;;                             void  OS_CPU_FP_Reg_Pop (OS_STK  *stkPtr)
;;
;; Note(s) : 1) This function restores S16-S31 of the Floating Point Unit.
;;
;;           2) Pseudo-code is:
;;              a) Restore regs S16-S31 of new process stack;
;;              b) Update OSTCBHighRdy->OSTCBStkPtr pointer of new proces stack;
;;********************************************************************************************************

    ;IF {FPU} != "SoftVFP"

;OS_CPU_FP_Reg_Pop
	;MRS R1,PSP
	;CBZ R1,OS_CPU_FP_nopop
	
    ;VLDMIA  R0!, {S16-S31}
    ;LDR     R1, =OSTCBHighRdy
    ;LDR     R2, [R1]
    ;STR     R0, [R2]
;OS_CPU_FP_nopop
	;BX      LR

    ;ENDIF


;;********************************************************************************************************
;;                                   CRITICAL SECTION METHOD 3 FUNCTIONS
;;
;; Description : Disable/Enable Kernel aware interrupts by preserving the state of BASEPRI.  Generally speaking,
;;               the state of the BASEPRI interrupt exception processing is stored in the local variable
;;               'cpu_sr' & Kernel Aware interrupts are then disabled ('cpu_sr' is allocated in all functions
;;               that need to disable Kernel aware interrupts). The previous BASEPRI interrupt state is restored
;;               by copying 'cpu_sr' into the BASEPRI register.
;;
;; Prototypes  : OS_CPU_SR  OS_CPU_SR_Save   (OS_CPU_SR  new_basepri);
;;               void       OS_CPU_SR_Restore(OS_CPU_SR  cpu_sr);
;;
;;
;; Note(s)     : 1) These functions are used in general like this:
;;
;;                  void Task (void *p_arg)
;;                  {
;;                  #if OS_CRITICAL_METHOD == 3          /* Allocate storage for CPU status register  */
;;                      OS_CPU_SR  cpu_sr;
;;                  #endif
;;
;;                          :
;;                          :
;;                      OS_ENTER_CRITICAL();             /* cpu_sr = OS_CPU_SR_Save(new_basepri);     */
;;                          :
;;                          :
;;                      OS_EXIT_CRITICAL();              /* OS_CPU_RestoreSR(cpu_sr);                 */
;;                          :
;;                          :
;;                  }
;;
;;               2) Increasing priority using a write to BASEPRI does not take effect immediately.
;;                  (a) IMPLICATION  This erratum means that the instruction after an MSR to boost BASEPRI
;;                      might incorrectly be preempted by an insufficient high priority exception.
;;
;;                  (b) WORKAROUND  The MSR to boost BASEPRI can be replaced by the following code sequence:
;;
;;                      CPSID i
;;                      MSR to BASEPRI
;;                      DSB
;;                      ISB
;;                      CPSIE i
;;********************************************************************************************************

;;OS_CPU_SR_Save
	;;CPSID I                 ;关闭中断，I位只是关中断，F位只是关快中断，IF则是中断和快中断都关，但是在ARM cortex-M系列由于提供了另一种机制
							;;;所以去除了FIQ模式，也就不需要都关
	;;PUSH {R1}				;压栈，我们使用R1作为一个中介
	;;MRS R1,BASEPRI			;当前状态(被中断的任务/被嵌套的中断)中BASEPRI,与S3C2440直接保存PSR不同
	;;MSR BASEPRI,R0			;由于我们传参进来，所以需要通过R0把参数给BASEPRI
	;;DSB						
	;;ISB
	;;MOV R0,R1				;把原来状态的BASEPRI作为返回值传给当前中断的cpu_sr
	;;POP {R1}				;出栈
	;;CPSIE I					;开中断
	;;BX LR					;返回执行
	
;;OS_CPU_SR_Restore
	;;CPSID I
	;;MSR BASEPRI,R0			;这里我们使用当前中断的cpu_sr作为参数传递以恢复之前的BASEPRI，与S3C2440恢复PSR不同
	;;DSB
	;;ISB
	;;CPSIE I
	;;BX LR
;OS_CPU_SR_Save
    ;MRS     R0, PRIMASK                                         ; Set prio int mask to mask all (except faults)
    ;CPSID   I
    ;BX      LR

;OS_CPU_SR_Restore
    ;MSR     PRIMASK, R0
    ;BX      LR

;;********************************************************************************************************
;;                                         START MULTITASKING
;;                                      void OSStartHighRdy(void)
;;
;; Note(s) : 1) This function triggers a PendSV exception (essentially, causes a context switch) to cause
;;              the first task to start.
;;
;;           2) During task execution, PSP is used as the stack pointer.
;;              When an exception occurs, the core will switch to MSP until the exception return.
;;
;;           3) OSStartHighRdy() MUST:
;;              a) Setup PendSV exception priority to lowest;
;;              b) Set initial PSP to 0, to tell context switcher this is first run;
;;              c) Set the main stack to OS_CPU_ExceptStkBase
;;              d) Set OSRunning to TRUE;
;;              e) Get current high priority, OSPrioCur = OSPrioHighRdy;
;;              f) Get current ready thread TCB, OSTCBCur = OSTCBHighRdy;
;;              g) Get new process SP from TCB, SP = OSTCBHighRdy->OSTCBStkPtr;
;;              h) Restore R0-R11 and R14 from new process stack;
;;              i) Enable interrupts (tasks will run with interrupts enabled).
;;********************************************************************************************************

;OSStartHighRdy
	
;;初始化MSP.PSP.PendSV的优先级，定义PendSV为最低优先级
;;这里解释一下，由于CortexM4里面的优先级寄存器分为了抢占和响应两种，共用四位
	;LDR R0,=NVIC_SYSPRI14
	;MOV R1,#0xFF
	;STRB R1,[R0] ;定义抢占优先级为15，也就是所能分配的最低优先级
	
	;;初始化PSP为0，以告知Switcher这是第一次任务开始
	;MOV R0,#0
	;MSR PSP,R0
	
	;;初始化MSP为OS_CPU_ExceptStkBase
	;LDR R0,=OS_CPU_ExceptStkBase
	;LDR R0,[R0]
	;MSR MSP,R0
	
	;BL OSTaskSwHook ;这条指令其实本来带有更新MSP的作用，但是我们其实没有必要去进行这条指令做一些无所谓的跳转和操作，所以这里需要自己手动更新PSP
	;;因为里面有一个FP_POP这会更新当前使用的栈指针
	
	;;设置OSRunning为TRUE
	;LDR R0, =OSRunning
	;MOV R1, #1
	;STRB R1, [R0]
	
	;;我们在前面的步骤已经确定了OSPrioCur和OSTCBCur
	;;我们取出我们自己定义的栈的地址赋给PSP，以作为接下来的操作的栈指针
	;LDR R0,=OSTCBCur
	;LDR R0,[R0]
	;LDR R0,[R0]
	;MSR PSP,R0
	
	;;由于正常情况下两种程序运行模式都是默认使用MSP，但是正常程序运行最好使用PSP
	;;默认上电为特权模式，只要不去特意改变，就会一直处在特权模式
	;MRS R0,CONTROL
	;ORR R0,R0,#0x02 ;使用PSP
	;MSR CONTROL,R0
	;ISB
	;;从这里开始，我们就正式开始使用PSP指针了
	;;初始化之后由于是初次开始，要进行以此特殊的上下文切换
	;;让第一个任务开始执行，这个函数不是异常，所以硬件不会自动帮我们压栈，需要在这里直接手动进行压栈
	;LDMIA SP!,{R4-R11}
	;LDMIA SP!,{R0-R3}
	;LDMIA SP!,{R12,LR}
	;LDMIA SP!,{R1,R2};R1存储了PC，R2存储了CPSR的值
	;;按照我的理解这里甚至只需要把PC读出来就行了。没有这么多繁琐的步骤，因为我们对第一个任务没有什么特殊的需求
	
	;VLDMIA SP!,{S16-S31}
	
	;;PC赋值
	;BX R1 ;由于在最开始的时候我们模拟压栈时就确定了程序入口，所以出栈给PC可以直接开始执行，由于正常函数执行也不是异常，所以选出的程序处于Thread模式下的特权模式


;;********************************************************************************************************
;;                       PERFORM A CONTEXT SWITCH (From task level) - OSCtxSw()
;;                   PERFORM A CONTEXT SWITCH (From interrupt level) - OSIntCtxSw()
;;
;; Note(s) : 1) OSCtxSw() is called when OS wants to perform a task context switch.  This function
;;              triggers the PendSV exception which is where the real work is done.
;;
;;           2) OSIntCtxSw() is called by OSIntExit() when it determines a context switch is needed as
;;              the result of an interrupt.  This function simply triggers a PendSV exception which will
;;              be handled when there are no more interrupts active and interrupts are enabled.
;;********************************************************************************************************

;OSCtxSw
;OSIntCtxSw
    ;LDR     R0, =NVIC_INT_CTRL                                  ; Trigger the PendSV exception (causes context switch)
    ;LDR     R1, =NVIC_PENDSVSET
    ;STR     R1, [R0]
	;DSB		
	;ISB
    ;BX      LR


;;********************************************************************************************************
;;                                       HANDLE PendSV EXCEPTION
;;                                   void OS_CPU_PendSVHandler(void)
;;
;; Note(s) : 1) PendSV is used to cause a context switch.  This is a recommended method for performing
;;              context switches with Cortex-M.  This is because the Cortex-M auto-saves half of the
;;              processor context on any exception, and restores same on return from exception.  So only
;;              saving of R4-R11 & R14 is required and fixing up the stack pointers. Using the PendSV exception
;;              this way means that context saving and restoring is identical whether it is initiated from
;;              a thread or occurs due to an interrupt or exception.
;;
;;           2) Pseudo-code is:
;;              a) Get the process SP
;;              b) Save remaining regs r4-r11 & r14 on process stack;
;;              c) Save the process SP in its TCB, OSTCBCur->OSTCBStkPtr = SP;
;;              d) Call OSTaskSwHook();
;;              e) Get current high priority, OSPrioCur = OSPrioHighRdy;
;;              f) Get current ready thread TCB, OSTCBCur = OSTCBHighRdy;
;;              g) Get new process SP from TCB, SP = OSTCBHighRdy->OSTCBStkPtr;
;;              h) Restore R4-R11 and R14 from new process stack;
;;              i) Perform exception return which will restore remaining context.
;;
;;           3) On entry into PendSV handler:
;;              a) The following have been saved on the process stack (by processor):
;;                 xPSR, PC, LR, R12, R0-R3
;;              b) Processor mode is switched to Handler mode (from Thread mode)
;;              c) Stack is Main stack (switched from Process stack)
;;              d) OSTCBCur      points to the OS_TCB of the task to suspend
;;                 OSTCBHighRdy  points to the OS_TCB of the task to resume
;;
;;           4) Since PendSV is set to lowest priority in the system (by OSStartHighRdy() above), we
;;              know that it will only be run when no other exception or interrupt is active, and
;;              therefore safe to assume that context being switched out was using the process stack (PSP).
;;
;;           5) Increasing priority using a write to BASEPRI does not take effect immediately.
;;              (a) IMPLICATION  This erratum means that the instruction after an MSR to boost BASEPRI
;;                  might incorrectly be preempted by an insufficient high priority exception.
;;
;;              (b) WORKAROUND  The MSR to boost BASEPRI can be replaced by the following code sequence:
;;
;;                  CPSID i
;;                  MSR to BASEPRI
;;                  DSB
;;                  ISB
;;                  CPSIE i
;;********************************************************************************************************

;OS_CPU_PendSVHandler ;由于不论是主动切换还是被动切换，都会经过这个PendSV来进行处理，并且通过资料我们可以知道，进入中断之前我们的硬件就已经把相关寄存器存入了被中断的例程所使用的指针所指向的内存
	;MRS R0,PSP
	;STMFD R0!,{R4-R11,LR};保存剩余现场,这里就包含了EXC_RETURN，在退出异常时，可以确定接下来的模式和栈指针
	
	;;LDR R2,=OSTCBCur
	;;LDR R1,[R2]
	;;STR R0,[R1];当前状态下，R0就是SP，需要存入TCB块
	
	;;在这里维护EXC_RETURN
	;MOV R4,LR
	;BL OSTaskSwHook
	;ORR LR,R4,#0x04 ;保证异常返回使用PSP
	
	;;然后切换OSTCBCur
	;;①进行OSPrioCur的切换，由于我们之前已经通过OS_SchedNew()改变了当前的OSPrioHighRdy
	;;OSPrioCur = OSPrioHighRdy
	;LDR R0,=OSPrioCur
	;LDR R1,=OSPrioHighRdy
	;LDRB R3,[R1];为什么在此处用LDRB而不是LDR呢，主要是因为优先级都是INT8U，为了防止其扩展为32位读取其他区域，我们采取字节的方式写入
	;STRB R3,[R0];同理
	
	;;②我们进行OSTCBCur的改变，同样此处的OSTCBHighRdy已经在OS_Sched()的前面步骤中获得
	;;但是为了后续的切换，每一次还是需要更新OSTCBCur的
	;LDR R0,=OSTCBCur
	;LDR R1,=OSTCBHighRdy
	;LDR R2,[R1]
	;STR R2,[R0]
	
	;;然后获取当前最高优先级任务的栈指针
	;LDR R0,[R0]
	;LDR R0,[R0]
	
	;;然后正式开始恢复现场，也就是接下来的任务的现场恢复
	;LDMIA R0!,{R4-R11,LR};异常中也就不需要进行对剩余的8个寄存器进行手动出栈了，硬件帮助我们完成
	;VLDMIA R0!,{S16-S31}
	;MSR PSP,R0;关于这里为什么不存一下到OSTCBCur->OSTCBStkPtr，可以看到我们上面进入的时候就是直接使用的PSP，也没有找OSTCBCur的首个字段值，所以我们在这里也就不需要去存
			  ;;在后续的操作结束后呢，我们默认其PSP恰好回到了我们恢复时的位置
	
	;BX LR
	
    ;ALIGN                                                       ; Removes warning[A1581W]: added <no_padbytes> of padding at <address>

    ;END
